/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "WirelessPositionLog.h"


CWirelessPositionLog::CWirelessPositionLog(std::string wirelessLogFile, std::string positionLogFile)
		:wirelessData(),
		positionData(),
		iPositionBacklog(0),
		wTimes(),
		pTimes(),
		signalMap()
															 
{
		if(readWirelessFile(wirelessLogFile) && readPositionLogFile(positionLogFile)){
				dPrint(1,"Succesfully read log files");
				dPrint(1,"There is %d Wireless Measurements and %d Position measurements",wirelessData.size(),positionData.size());
		}else{
				dPrint(1,"Failed to read log files - abort");
				exit(1);
		}
		dPrint(1,"Creating ID map");
		createIDMap();
}

CWirelessPositionLog::~CWirelessPositionLog(){
}	

/**
* Reads the Wireless log file to a container
*/
bool CWirelessPositionLog::readWirelessFile(std::string wirelessLogFile){
		using namespace MaCI::Wireless;
		MaCI::Common::TTimestamp timestamp;
		uint64_t nodeIDSignal, nodeIDRange, nodeIDPosition, nodeIDESSID;;
		FILE *iFile;
		float range, rangeVar,posX,posY,posZ;
		char ESSID[32];
		int signalLevel,signalCount, rangeCount, positionCount, ESSIDCount;

		bool first = true;
		int number = 1;
		
		dPrint(3,"Opening file '%s'",wirelessLogFile.c_str());
		iFile= fopen(wirelessLogFile.c_str(),"rt");
		if(iFile == NULL){
				dPrint(1,"Unable to open log file '%s'",wirelessLogFile.c_str());
				return false;
		}
		char c[200];
		char interfaceName[64];
		//Read the four first lines..and get the interfacename...
		fgets (c, 200,iFile );

		fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
		fgets (c, 200,iFile );
		fgets (c, 200,iFile );
		
		dPrint(1,"Log is recorded from %s",interfaceName);
		gim::time iBegin;
		gim::time iEnd;
			
		while(number != 0 && number != EOF){
				CWirelessData *wData = new CWirelessData();
				wData->CreateInternalBinBag();
   
				///Check for timestamp and signalcount
				number = fscanf(iFile,"%u %u %d",
												&(timestamp.timestamp_s),
													&(timestamp.timestamp_us),
														&signalCount);

				if(number >0){
						wData->SetTimestamp(timestamp);
				}else{
						break;
				}
   
				///Save the first timestamp
				if(first){
						iBegin = timestamp.GetGimTime();
						first = false;
				}

				///Get all signal levels and save them to wireless data
				for(int i =  0; i< signalCount; i++){
						number = fscanf(iFile,"%llx %d",
														&nodeIDSignal, &signalLevel);

						if(number > 0){
								wData->AddNodeSignal(TNodeSignal(nodeIDSignal, signalLevel));
						}else{
								break;
						}
				}

				///Get range count
				number = fscanf(iFile,"%d",
												&rangeCount);

				///Get all ranges and save them to wireless data
				for(int i =  0; i< rangeCount; i++){
						number = fscanf(iFile,"%llx %f %f",
														&nodeIDRange,&range, &rangeVar);
						if(number > 0){
								wData->AddNodeRange(TNodeRange(nodeIDRange, range, rangeVar));
        
						}else{
								break;
						}     

      
				}
				///Get position count
				number = fscanf(iFile,"%d",
												&positionCount);

				///Get all positions and save them to wireless data
				for(int i =  0; i< positionCount; i++){
						number = fscanf(iFile,"%llx %f %f %f",
														&nodeIDPosition,&posX,&posY,&posZ);
						if(number > 0){
								wData->AddNodePosition(TNodePosition(nodeIDPosition, posX,posY,posZ));
						}else{
								break;
						}     
        
				}

				///Get id count
				number = fscanf(iFile,"%d",
												&ESSIDCount);

				///Get all ids and save them to wireless data
				for(int i =  0; i< ESSIDCount; i++){
						number = fscanf(iFile,"%llx '%s",
														&nodeIDESSID,ESSID);
						if(number > 0){
								wData->AddNodeID(TNodeID(nodeIDESSID,ESSID));
						}else{
								break;
						}     
        
				}

				///Finally add wirelss data to map and timestamp to list
				if(wData){
						//wirelessData[timestamp.GetGimTime()] = wData;
						wirelessData.push_back(wData);
						wTimes.push_back(timestamp.GetGimTime());
				}
		}

		iEnd = timestamp.GetGimTime();

		if(number == EOF){
				return true;
		}else{
				return false;
		}

}

bool CWirelessPositionLog::readPositionLogFile( std::string positionLogFile){
		FILE *iFile;
		dPrint(3,"Opening file '%s'",positionLogFile.c_str());
		iFile= fopen(positionLogFile.c_str(),"rt");
		if(iFile == NULL){
					dPrint(1,"Unable to open Log file '%s'",positionLogFile.c_str());
				return false;
		}
		char c[200];
		char interfaceName[64];
  //Read the four first lines..and get the interfacename...
		fgets (c, 200,iFile );

		fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
		fgets (c, 200,iFile );
		fgets (c, 200,iFile );
		dPrint(1,"Log is recorded from %s",interfaceName);
		gim::time iBegin;
		gim::time iEnd;
		
		using namespace MaCI::Position;
		MaCI::Common::TTimestamp timestamp;
		float posX,posY,posA,varX,varY,varA,prob;
		int number = 1;
		bool first = true;
		while(number != 0 && number != EOF){
    
				number = fscanf(iFile,"%u %u %f %f %f %f %f %f %f\n",
												&(timestamp.timestamp_s),
													&(timestamp.timestamp_us),
														&posX,&posY,&posA,
														&varX,&varY,&varA,
														&prob);
				if(first){
						iBegin = timestamp.GetGimTime();
						first = false;
				}
				if(number> 0){
						CPositionData *pos = new CPositionData();
						pos->CreateInternalBinBag();
						pos->SetTimestamp(timestamp);
						pos->SetPose2D(TPose2D(posX,posY,posA));
						pos->SetVariance2D(TVariance2D(varX,varY,varA));
						pos->SetProbability(TProbability(prob));
      //pos->Print(1);
						positionData[timestamp.GetGimTime()] = pos;
						iPositionBacklog.PushData(timestamp.GetGimTime(), *pos, false);
						pTimes.push_back(timestamp.GetGimTime());
				}
		}
		iEnd = timestamp.GetGimTime();
		if(number == EOF){
				return true;
		}else{
				return false;
		}
		
 		return true;
}


bool CWirelessPositionLog::createIDMap(){
		
		MaCI::Position::CPositionData pos;
		
		for(EACH_IN_i(wirelessData)){
				iPositionBacklog.PeekData(pos, (*i)->GetTimestamp()->GetGimTime(), NULL);
				///Retreive the ID and Signal Strength values
				MaCI::Wireless::TNodeIDPtrArray idArray = (*i)->GetNodeIDArray();
				MaCI::Wireless::TNodeSignalPtrArray signalArray = (*i)->GetNodeSignalArray();
				
				///Retreive pose
				const MaCI::Position::TPose2D *p = pos.GetPose2D();
				
				for(unsigned int j=0;j<idArray.size();j++){
						signalMap.insert(std::pair<uint64_t, SignalStrengthWithPosition>( idArray[j]->nodeid,
														 SignalStrengthWithPosition(signalArray[j]->signal_level,p->x,p->y, idArray[j]->ESSID))); 
				}
				
				//dPrint(1,"There is %d measurements recorded at (%.2f, %.2f)",idArray.size(),p->x,p->y);
				
				//(*i)->Print(1);
				//pos.Print(1);
		} 
		std::multimap<uint64_t, SignalStrengthWithPosition>::iterator ii;
		ii=signalMap.begin();
		uint64_t key = (*ii).first;
		signalKeys.push_back(key);
		for(ii=signalMap.begin(); ii!=signalMap.end(); ii++)
		{
				if(key != (*ii).first){
						key = (*ii).first;
						signalKeys.push_back(key);
				} 
		}
		
		
		return true;
}

std::string CWirelessPositionLog::getESSID(uint64_t key){
	std::pair<std::multimap<uint64_t, SignalStrengthWithPosition>::iterator, 
	std::multimap<uint64_t, SignalStrengthWithPosition>::iterator> partial;
	partial = signalMap.equal_range(key);	
	std::multimap<uint64_t, SignalStrengthWithPosition>::iterator it2 = partial.first;
	
	return (*it2).second.ESSID;
}

/**
 * Returns the number of measurements on one MAC key
 */
int CWirelessPositionLog::getNumMeasurements(uint64_t key){
	return signalMap.count(key);
}

/***************************************************************************************
***************************************************************************************/

//#define WIRELESSPOSITION_DEBUG
#ifdef WIRELESSPOSITION_DEBUG

int main(void){
		
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(4);
		
		CWirelessPositionLog log("Room2534.3JBO.MaCI_Wireless.Scanner_17_06_2009_102220.txt", 
														 "Room2534.3JBO.MaCI_Position.Dummy_17_06_2009_102220.txt");
		MaCI::Position::CPositionData pos;
		
		///Read the data and match Wireless timestamp to position timestamp
		for(EACH_IN_i(log.wirelessData)){
				log.iPositionBacklog.PeekData(pos, (*i)->GetTimestamp()->GetGimTime(), NULL);
				///Retreive the ID and Signal Strength values
				MaCI::Wireless::TNodeIDPtrArray idArray = (*i)->GetNodeIDArray();
				MaCI::Wireless::TNodeSignalPtrArray signalArray = (*i)->GetNodeSignalArray();
				
				///Retreive pose
				const MaCI::Position::TPose2D *p = pos.GetPose2D();
				
				dPrint(10,"There is %d measurements recorded at (%.2f, %.2f)",
							 																					idArray.size(),p->x,p->y);
				(*i)->Print(10);
				pos.Print(10);
		} 
		
		///Print all multimap elements
		std::multimap<uint64_t, SignalStrengthWithPosition>::iterator ii;
		for(ii=log.signalMap.begin(); ii!=log.signalMap.end(); ii++)
		{
				dPrint(10,"%llx %f %f",(*ii).first, (*ii).second.x,(*ii).second.y);
		}
		
		dPrint(1,"Number of Signal elements = %d, number of keys = %d",
					 													log.signalMap.size(),log.signalKeys.size());
		
		///Count components key-wise
		for(unsigned int i=0; i<log.signalKeys.size();i++){
				dPrint(1,"ID=%llx :: number of elements is %d",
							 		log.signalKeys[i], log.signalMap.count(log.signalKeys[i]));
		}
		
		///Iterate values using the groups specified by the keys
		std::pair<std::multimap<uint64_t, SignalStrengthWithPosition>::iterator, 
														std::multimap<uint64_t, SignalStrengthWithPosition>::iterator> partial;
														
		for(unsigned int i=0; i<log.signalKeys.size();i++){
				partial = log.signalMap.equal_range(log.signalKeys[i]);
				
				for (std::multimap<uint64_t, SignalStrengthWithPosition>::iterator it2 = partial.first;
								 it2 != partial.second; ++it2)
				{
						dPrint(1,"%llx=%s %f %f",(*it2).first,(*it2).second.ESSID.c_str(), (*it2).second.x,(*it2).second.y);
						
				}
		}
		
		return 0;
}

#endif

